' SumoBot_2.2_Motor_Test.BS2
' {$STAMP BS2}
' {$PBASIC 2.5}

' -----[ I/O Definitions ]-----------------------------------

LMotor          PIN     13        ' left servo motor
RMotor          PIN     12        ' right servo motor

' -----[ Constants ]------------------------------------------

LFwdFast        CON    1000       ' left motor fwd; fast
LFwdSlow        CON     800       ' left motor fwd; slow
LStop           CON     750       ' left motor stop
LRevSlow        CON     700       ' left motor rev; slow
LRevFast        CON     500       ' left motor rev; fast

RFwdFast        CON     500       ' right motor fwd; fast
RFwdSlow        CON     700       ' right motor fwd; slow
RStop           CON     750       ' right motor stop
RRevSlow        CON     800       ' right motor rev; slow
RRevFast        CON    1000       ' right motor rev; fast

' -----[ Variables ]-----------------------------------------

pulses          VAR    Byte       ' servo pulses counter

' -----[ Initialization ]-------------------------------------

Reset:
  LOW LMotor                      ' initialize motor outputs
  LOW RMotor
  PAUSE 2000                      ' time to disconnect cable

' -----[ Program Code ]---------------------------------------

Main:
  FOR pulses = 1 TO 65            ' fwd slow, ~12 inches
    PULSOUT LMotor, LFwdSlow
    PULSOUT RMotor, RFwdSlow
    PAUSE 20
  NEXT

  FOR pulses = 1 TO 30            ' pivot 90 on left wheel
    PULSOUT LMotor, LStop
    PULSOUT RMotor, RFwdSlow
    PAUSE 20
  NEXT

  FOR pulses = 1 TO 25
    PULSOUT LMotor, LFwdFast      ' fwd fast
    PULSOUT RMotor, RFwdFast
    PAUSE 20
  NEXT

  FOR pulses = 1 TO 60
    PULSOUT LMotor, LFwdSlow      ' pivot 180 on right wheel
    PULSOUT RMotor, RStop
    PAUSE 20
  NEXT

  FOR pulses = 1 TO 50            ' fwd fast
    PULSOUT LMotor, LFwdFast
    PULSOUT RMotor, RFwdFast
    PAUSE 20
  NEXT

  FOR pulses = 1 TO 55            ' spin turn - clockwise
    PULSOUT LMotor, LFwdFast
    PULSOUT RMotor, RRevFast
    PAUSE 20
  NEXT

Hold_Position:
  DO
    PULSOUT LMotor, LStop
    PULSOUT RMotor, RStop
    PAUSE 20
  LOOP

  END